// This file is distributed under a BSD license. See LICENSE.txt for details.

#include "_intmath.hpp"

/****************************************************************************/

#if sPLATFORM==sPLAT_PDA && sMOBILE

//

#else

sInt sCountLeadingZeroes(sU32 x)
{
  if(!x)
    return 32;

  static const sU8 tailTable[16] = { 3,3,2,2, 1,1,1,1, 0,0,0,0, 0,0,0,0 };

  sInt lz = 0;
  if(x & 0xffff0000) x >>= 16; else lz += 16;
  if(x & 0x0000ff00) x >>=  8; else lz +=  8;
  if(x & 0x000000f0) x >>=  4; else lz +=  4;
  return lz + tailTable[x];
}

#endif

/****************************************************************************/

#define SINTABSHIFT (5)
#define SINTABMASK ((1<<SINTABSHIFT)-1)
#define SINTABSIZE ((sFIXONE/4)>>SINTABSHIFT)

static sU16 SinTab[SINTABSIZE+1] = 
{
  0x0000,0x0192,0x0324,0x04b6,0x0648,0x07d9,0x096b,0x0afb,
  0x0c8c,0x0e1c,0x0fab,0x113a,0x12c8,0x1455,0x15e2,0x176e,
  0x18f9,0x1a83,0x1c0c,0x1d93,0x1f1a,0x209f,0x2224,0x23a7,
  0x2528,0x26a8,0x2827,0x29a4,0x2b1f,0x2c99,0x2e11,0x2f87,
  0x30fc,0x326e,0x33df,0x354e,0x36ba,0x3825,0x398d,0x3af3,
  0x3c57,0x3db8,0x3f17,0x4074,0x41ce,0x4326,0x447b,0x45cd,
  0x471d,0x486a,0x49b4,0x4afb,0x4c40,0x4d81,0x4ec0,0x4ffb,
  0x5134,0x5269,0x539b,0x54ca,0x55f6,0x571e,0x5843,0x5964,
  0x5a82,0x5b9d,0x5cb4,0x5dc8,0x5ed7,0x5fe4,0x60ec,0x61f1,
  0x62f2,0x63ef,0x64e9,0x65de,0x66d0,0x67bd,0x68a7,0x698c,
  0x6a6e,0x6b4b,0x6c24,0x6cf9,0x6dca,0x6e97,0x6f5f,0x7023,
  0x70e3,0x719e,0x7255,0x7308,0x73b6,0x7460,0x7505,0x75a6,
  0x7642,0x76d9,0x776c,0x77fb,0x7885,0x790a,0x798a,0x7a06,
  0x7a7d,0x7aef,0x7b5d,0x7bc6,0x7c2a,0x7c89,0x7ce4,0x7d3a,
  0x7d8a,0x7dd6,0x7e1e,0x7e60,0x7e9d,0x7ed6,0x7f0a,0x7f38,
  0x7f62,0x7f87,0x7fa7,0x7fc2,0x7fd9,0x7fea,0x7ff6,0x7ffe,
  0x8000,
};

void sInitIntMath2()
{
  /*
  sInt i;
  for(i=0;i<=SINTABSIZE;i++)
  {
    if((i&7)==0) sDPrintF("\n  ");
    SinTab[i] = (sInt)(sFSin(i*(sPIF/2)/SINTABSIZE)*sFIXONE*2+0.5f);
    sDPrintF("0x%04x,",SinTab[i]);
  }
  sDPrintF("\n");
*/
  /*
  sF32 maxerr = 0;
  for(i=0;i<0x4000;i++)
  {
    sInt a = i;
    sF32 r0 = sISin(a);
    sF32 r1 = sFSin(a*sPI2F/sFIXONE)*sFIXONE;
    sF32 err = r0-r1;
    if(sFAbs(err)>maxerr)maxerr=sFAbs(err);
//    sDPrintF("%6d -> %f %f : %f\n",a,r0,r1,err);
  }
  sDPrintF("maxerr sin: %f\n",maxerr);
  sDPrintF("..\n");
  */
}

sInt sISin(sInt a)
{
  a = a&0x3fff;
  if(a<0x2000)
  {
    if(a>=0x1000)
      a = 0x2000-a;

    sInt i = a>>(SINTABSHIFT);
    sInt f = a&SINTABMASK;
    return (SinTab[i] + (((SinTab[i+1]-SinTab[i])*f)>>SINTABSHIFT))/2;
  }
  else
  {
    a = a-0x2000;
    if(a>=0x1000)
      a = 0x2000-a;

    sInt i = a>>(SINTABSHIFT);
    sInt f = a&SINTABMASK;
    return -(SinTab[i] + (((SinTab[i+1]-SinTab[i])*f)>>SINTABSHIFT))/2;
  }
}


sInt sICos(sInt a)
{
  return sISin(a+sFIXONE/4);
}

static unsigned short SqrtTab[257] =
{
  0x0000,0x007f,0x00ff,0x017e,0x01fe,0x027c,0x02fb,0x0379,0x03f8,0x0476,0x04f3,0x0571,0x05ee,0x066b,0x06e8,0x0764,
  0x07e0,0x085d,0x08d8,0x0954,0x09cf,0x0a4b,0x0ac5,0x0b40,0x0bbb,0x0c35,0x0caf,0x0d29,0x0da3,0x0e1c,0x0e95,0x0f0e,
  0x0f87,0x1000,0x1078,0x10f0,0x1168,0x11e0,0x1257,0x12cf,0x1346,0x13bd,0x1433,0x14aa,0x1520,0x1596,0x160c,0x1682,
  0x16f8,0x176d,0x17e2,0x1857,0x18cc,0x1941,0x19b5,0x1a29,0x1a9d,0x1b11,0x1b85,0x1bf8,0x1c6c,0x1cdf,0x1d52,0x1dc4,
  0x1e37,0x1ea9,0x1f1c,0x1f8e,0x2000,0x2071,0x20e3,0x2154,0x21c5,0x2236,0x22a7,0x2318,0x2388,0x23f8,0x2469,0x24d9,
  0x2548,0x25b8,0x2628,0x2697,0x2706,0x2775,0x27e4,0x2852,0x28c1,0x292f,0x299e,0x2a0c,0x2a79,0x2ae7,0x2b55,0x2bc2,
  0x2c2f,0x2c9c,0x2d09,0x2d76,0x2de3,0x2e4f,0x2ebb,0x2f28,0x2f94,0x3000,0x306b,0x30d7,0x3142,0x31ad,0x3219,0x3284,
  0x32ee,0x3359,0x33c4,0x342e,0x3498,0x3502,0x356c,0x35d6,0x3640,0x36a9,0x3713,0x377c,0x37e5,0x384e,0x38b7,0x3920,
  0x3988,0x39f1,0x3a59,0x3ac1,0x3b29,0x3b91,0x3bf9,0x3c61,0x3cc8,0x3d30,0x3d97,0x3dfe,0x3e65,0x3ecc,0x3f32,0x3f99,
  0x4000,0x4066,0x40cc,0x4132,0x4198,0x41fe,0x4264,0x42c9,0x432f,0x4394,0x43f9,0x445e,0x44c3,0x4528,0x458d,0x45f1,
  0x4656,0x46ba,0x471e,0x4783,0x47e7,0x484a,0x48ae,0x4912,0x4975,0x49d9,0x4a3c,0x4a9f,0x4b02,0x4b65,0x4bc8,0x4c2b,
  0x4c8d,0x4cf0,0x4d52,0x4db4,0x4e16,0x4e79,0x4eda,0x4f3c,0x4f9e,0x5000,0x5061,0x50c2,0x5124,0x5185,0x51e6,0x5247,
  0x52a7,0x5308,0x5369,0x53c9,0x542a,0x548a,0x54ea,0x554a,0x55aa,0x560a,0x566a,0x56c9,0x5729,0x5788,0x57e8,0x5847,
  0x58a6,0x5905,0x5964,0x59c3,0x5a22,0x5a80,0x5adf,0x5b3d,0x5b9b,0x5bfa,0x5c58,0x5cb6,0x5d14,0x5d71,0x5dcf,0x5e2d,
  0x5e8a,0x5ee8,0x5f45,0x5fa2,0x6000,0x605d,0x60b9,0x6116,0x6173,0x61d0,0x622c,0x6289,0x62e5,0x6341,0x639e,0x63fa,
  0x6456,0x64b2,0x650d,0x6569,0x65c5,0x6620,0x667c,0x66d7,0x6732,0x678e,0x67e9,0x6844,0x689f,0x68f9,0x6954,0x69af,
  0x6a09,
};


__forceinline sInt Mul24(sInt var_a,sInt var_b)
{
  return (sS32)( ((sS64)var_a)*((sS64)var_b)>>24 );
}

sInt sIntSqrt15D(sU32 r)
{
  sU32 e = sCountLeadingZeroes(r);
  sU32 x = r<<(e+1);
  sU32 i = x>>24;
  sU32 f = (x&0x00ffffff);
  
  x =  SqrtTab[i] + Mul24(SqrtTab[i+1]-SqrtTab[i],f) + 0x10000;
  if(e&1) x=x>>1;
  else x=Mul24(x+1,11863283);
  return (x>>(e/2));
}

/****************************************************************************/

static unsigned int ISqrtTab[257] =
{
  0x40000000,0x3fe017ec,0x3fc05f61,0x3fa0d5e9,0x3f817b11,0x3f624e65,0x3f434f76,0x3f247dd4,0x3f05d910,0x3ee760be,0x3ec91474,0x3eaaf3c7,0x3e8cfe50,0x3e6f33a7,0x3e519367,0x3e341d2b,
  0x3e16d091,0x3df9ad37,0x3ddcb2bd,0x3dbfe0c3,0x3da336eb,0x3d86b4d9,0x3d6a5a31,0x3d4e2698,0x3d3219b5,0x3d163330,0x3cfa72b2,0x3cded7e4,0x3cc36271,0x3ca81207,0x3c8ce650,0x3c71defd,
  0x3c56fbbb,0x3c3c3c3c,0x3c21a02f,0x3c072747,0x3becd136,0x3bd29db2,0x3bb88c6d,0x3b9e9d1f,0x3b84cf7d,0x3b6b233f,0x3b51981c,0x3b382dcf,0x3b1ee411,0x3b05ba9d,0x3aecb12e,0x3ad3c780,
  0x3abafd52,0x3aa2525f,0x3a89c668,0x3a71592b,0x3a590a69,0x3a40d9e3,0x3a28c759,0x3a10d28e,0x39f8fb46,0x39e14143,0x39c9a44a,0x39b22421,0x399ac08b,0x39837951,0x396c4e38,0x39553f09,
  0x393e4b8b,0x39277387,0x3910b6c7,0x38fa1514,0x38e38e38,0x38cd2200,0x38b6d037,0x38a098a8,0x388a7b21,0x38747770,0x385e8d61,0x3848bcc3,0x38330565,0x381d6717,0x3807e1a9,0x37f274eb,
  0x37dd20ad,0x37c7e4c2,0x37b2c0fc,0x379db52c,0x3788c125,0x3773e4bc,0x375f1fc3,0x374a720f,0x3735db74,0x37215bc8,0x370cf2e1,0x36f8a093,0x36e464b6,0x36d03f21,0x36bc2faa,0x36a8362a,
  0x36945278,0x3680846c,0x366ccbe1,0x365928ae,0x36459aad,0x363221b9,0x361ebdac,0x360b6e60,0x35f833b0,0x35e50d79,0x35d1fb95,0x35befde1,0x35ac143a,0x35993e7b,0x35867c83,0x3573ce2f,
  0x3561335d,0x354eabea,0x353c37b5,0x3529d69e,0x35178883,0x35054d43,0x34f324bf,0x34e10ed5,0x34cf0b68,0x34bd1a56,0x34ab3b82,0x34996ecc,0x3487b415,0x34760b40,0x3464742f,0x3452eec3,
  0x34417ae0,0x34301867,0x341ec73d,0x340d8745,0x33fc5862,0x33eb3a78,0x33da2d6c,0x33c93121,0x33b8457c,0x33a76a63,0x33969fb9,0x3385e566,0x33753b4d,0x3364a156,0x33541765,0x33439d62,
  0x33333333,0x3322d8be,0x33128deb,0x330252a0,0x32f226c6,0x32e20a43,0x32d1fcff,0x32c1fee3,0x32b20fd7,0x32a22fc2,0x32925e8e,0x32829c24,0x3272e86d,0x32634351,0x3253acba,0x32442492,
  0x3234aac2,0x32253f35,0x3215e1d5,0x3206928b,0x31f75143,0x31e81de7,0x31d8f862,0x31c9e0a0,0x31bad68a,0x31abda0e,0x319ceb15,0x318e098d,0x317f3560,0x31706e7c,0x3161b4cb,0x3153083c,
  0x314468b9,0x3135d630,0x3127508e,0x3118d7bf,0x310a6bb2,0x30fc0c52,0x30edb98e,0x30df7353,0x30d1398f,0x30c30c30,0x30b4eb24,0x30a6d659,0x3098cdbe,0x308ad140,0x307ce0cf,0x306efc59,
  0x306123cd,0x3053571a,0x3045962f,0x3037e0fc,0x302a3770,0x301c997b,0x300f070b,0x30018012,0x2ff4047e,0x2fe69440,0x2fd92f47,0x2fcbd586,0x2fbe86ea,0x2fb14366,0x2fa40aea,0x2f96dd66,
  0x2f89bacc,0x2f7ca30c,0x2f6f9617,0x2f6293df,0x2f559c55,0x2f48af6b,0x2f3bcd11,0x2f2ef53a,0x2f2227d7,0x2f1564da,0x2f08ac35,0x2efbfddb,0x2eef59bd,0x2ee2bfcd,0x2ed62ffe,0x2ec9aa42,
  0x2ebd2e8d,0x2eb0bcd0,0x2ea454fe,0x2e97f70b,0x2e8ba2e8,0x2e7f588a,0x2e7317e3,0x2e66e0e7,0x2e5ab388,0x2e4e8fbb,0x2e427572,0x2e3664a2,0x2e2a5d3e,0x2e1e5f3a,0x2e126a89,0x2e067f20,
  0x2dfa9cf2,0x2deec3f4,0x2de2f41a,0x2dd72d58,0x2dcb6fa2,0x2dbfbaee,0x2db40f2f,0x2da86c5a,0x2d9cd263,0x2d914140,0x2d85b8e6,0x2d7a3948,0x2d6ec25d,0x2d635419,0x2d57ee72,0x2d4c915c,
  0x2d413ccc
};


sInt ISqrt15D(sU32 r)
{
  sU32 e = sCountLeadingZeroes(r);
  sU32 x = r<<(e+1);
  sU32 i = x>>24;
  sU32 f = (x&0x00ffffff);
  
  x =  ISqrtTab[i] + Mul24(ISqrtTab[i+1]-ISqrtTab[i],f);
  if(!(e&1) )
    x=Mul24(x+1,11863283);

  return (x>>(15-e/2));
}

void sIVector3::Unit()
{
  sInt e = sIMul(x,x)+sIMul(y,y)+sIMul(z,z);
  e = ISqrt15D(e)>>9;

  x = sIMul(x,e);
  y = sIMul(y,e);
  z = sIMul(z,e);
}

void sIVector3::Cross(const sIVector3 &a,const sIVector3 &b)
{
  x = sIMul(a.y,b.z) - sIMul(a.z,b.y); 
  y = sIMul(a.z,b.x) - sIMul(a.x,b.z); 
  z = sIMul(a.x,b.y) - sIMul(a.y,b.x);
}

void sIVector3::Rotate34(const sIMatrix34 &mat)
{
  sInt xx=x;
  sInt yy=y;
  sInt zz=z;

  x = sIMul(mat.i.x,xx) + sIMul(mat.j.x,yy) + sIMul(mat.k.x,zz) + mat.l.x;
  y = sIMul(mat.i.y,xx) + sIMul(mat.j.y,yy) + sIMul(mat.k.y,zz) + mat.l.y;
  z = sIMul(mat.i.z,xx) + sIMul(mat.j.z,yy) + sIMul(mat.k.z,zz) + mat.l.z;
}

/****************************************************************************/

void sISRT::Init()
{
  s.Init(sFIXONE,sFIXONE,sFIXONE);
  r.Init(0,0,0);
  t.Init(0,0,0);
}

void sISRT::Fix14()
{
  s.x >>= 1;
  s.y >>= 1;
  s.z >>= 1;
  r.x >>= 1;
  r.y >>= 1;
  r.z >>= 1;
  t.x >>= 1;
  t.y >>= 1;
  t.z >>= 1;
}

/****************************************************************************/

void sIMatrix34::Init()
{
  i.Init(sFIXONE,0,0);
  j.Init(0,sFIXONE,0);
  k.Init(0,0,sFIXONE);
  l.Init(0,0,0);
}

void sIMatrix34::InitEuler(sInt a,sInt b,sInt c)
{
  sInt sx,sy,sz;
  sInt cx,cy,cz;

  sx = sISin(a&0x3fff);
  sy = sISin(b&0x3fff);
  sz = sISin(c&0x3fff);
  cx = sICos(a&0x3fff);
  cy = sICos(b&0x3fff);
  cz = sICos(c&0x3fff);

  i.x =  (cy*cz)>>14;
  i.y =  (cy*sz)>>14;
  i.z = -sy;
  sInt sxsy = (sx*sy)>>14;
  j.x = (-cx*sz + sxsy*cz)>>14;
  j.y = ( cx*cz + sxsy*sz)>>14;
  j.z =  (sx*cy)>>14;
  sInt cxsy = (cx*sy)>>14;
  k.x = ( sx*sz + cxsy*cz)>>14;
  k.y = (-sx*cz + cxsy*sz)>>14;
  k.z =  (cx*cy)>>14;

  l.x = 0;
  l.y = 0;
  l.z = 0;
}

void sIMatrix34::InitSRT(sISRT &srt)
{
  InitEuler(srt.r.x,srt.r.y,srt.r.z);
  i.x = sIMul(i.x,srt.s.x);
  i.y = sIMul(i.y,srt.s.x);
  i.z = sIMul(i.z,srt.s.x);
  j.x = sIMul(j.x,srt.s.y);
  j.y = sIMul(j.y,srt.s.y);
  j.z = sIMul(j.z,srt.s.y);
  k.x = sIMul(k.x,srt.s.z);
  k.y = sIMul(k.y,srt.s.z);
  k.z = sIMul(k.z,srt.s.z);
  l = srt.t;
}

void sIMatrix34::Mul(const sIMatrix34 &a,const sIMatrix34 &b)
{
  i.x = sIMul(a.i.x,b.i.x) + sIMul(a.i.y,b.j.x) + sIMul(a.i.z,b.k.x);
  i.y = sIMul(a.i.x,b.i.y) + sIMul(a.i.y,b.j.y) + sIMul(a.i.z,b.k.y);
  i.z = sIMul(a.i.x,b.i.z) + sIMul(a.i.y,b.j.z) + sIMul(a.i.z,b.k.z);

  j.x = sIMul(a.j.x,b.i.x) + sIMul(a.j.y,b.j.x) + sIMul(a.j.z,b.k.x);
  j.y = sIMul(a.j.x,b.i.y) + sIMul(a.j.y,b.j.y) + sIMul(a.j.z,b.k.y);
  j.z = sIMul(a.j.x,b.i.z) + sIMul(a.j.y,b.j.z) + sIMul(a.j.z,b.k.z);

  k.x = sIMul(a.k.x,b.i.x) + sIMul(a.k.y,b.j.x) + sIMul(a.k.z,b.k.x);
  k.y = sIMul(a.k.x,b.i.y) + sIMul(a.k.y,b.j.y) + sIMul(a.k.z,b.k.y);
  k.z = sIMul(a.k.x,b.i.z) + sIMul(a.k.y,b.j.z) + sIMul(a.k.z,b.k.z);

  l.x = sIMul(a.l.x,b.i.x) + sIMul(a.l.y,b.j.x) + sIMul(a.l.z,b.k.x) + b.l.x;
  l.y = sIMul(a.l.x,b.i.y) + sIMul(a.l.y,b.j.y) + sIMul(a.l.z,b.k.y) + b.l.y;
  l.z = sIMul(a.l.x,b.i.z) + sIMul(a.l.y,b.j.z) + sIMul(a.l.z,b.k.z) + b.l.z;
}

void sIMatrix34::TransR()
{
  sSwap(i.y,j.x);
  sSwap(i.z,k.x);
  sSwap(j.z,k.y);

  sInt xx = -l.x;
  sInt yy = -l.y;
  sInt zz = -l.z;

  l.x = sIMul(i.x,xx) + sIMul(j.x,yy) + sIMul(k.x,zz);
  l.y = sIMul(i.y,xx) + sIMul(j.y,yy) + sIMul(k.y,zz);
  l.z = sIMul(i.z,xx) + sIMul(j.z,yy) + sIMul(k.z,zz);
}

/****************************************************************************/

